#include "fusb302.h"

bool is_cc_configured = FALSE;
volatile uint8_t msg_id = 0;
uint8_t fusb302_ufp_cc = 0;

void fusb302_init(void)
{
}

void fusb302_reset(void) {
    fusb302_tcpc_write(TCPC_REG_RESET, TCPC_REG_RESET_SW_RESET);
}

//Communication
void fusb302_tcpc_write(uint8_t reg, uint8_t val) {
    uint8_t data[] = {reg & 0xFF, val & 0xFF};
    IIC_WriteSlave(FUSB302_I2C_SLAVE_ADDR, data, 2);
}

uint8_t fusb302_tcpc_read(uint8_t reg) {
    uint8_t resu;
    IIC_ReadSlave(FUSB302_I2C_SLAVE_ADDR, reg & 0xFF, &resu, 1);
	return resu;
}

//Device ID
uint8_t fusb302_get_chip_id(void) {

    return fusb302_tcpc_read(TCPC_REG_DEVICE_ID);

}

//Switches 0
void fusb302_cc_pu_switch(bool cc1, bool cc2) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_SWITCHES0);

    if (cc1)
        reg |= TCPC_REG_SWITCHES0_CC1_PU_EN;
    else
        reg &= ~TCPC_REG_SWITCHES0_CC1_PU_EN;

    if (cc2)
        reg |= TCPC_REG_SWITCHES0_CC2_PU_EN;
    else
        reg &= ~TCPC_REG_SWITCHES0_CC2_PU_EN;

    fusb302_tcpc_write(TCPC_REG_SWITCHES0, reg);

}

void fusb302_cc_vconn_switch(bool cc1, bool cc2) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_SWITCHES0);

    if (cc1)
        reg |= TCPC_REG_SWITCHES0_VCONN_CC1;
    else
        reg &= ~TCPC_REG_SWITCHES0_VCONN_CC1;

    if (cc2)
        reg |= TCPC_REG_SWITCHES0_VCONN_CC2;
    else
        reg &= ~TCPC_REG_SWITCHES0_VCONN_CC2;

    fusb302_tcpc_write(TCPC_REG_SWITCHES0, reg);

}

void fusb302_cc_meas_switch(bool cc1, bool cc2) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_SWITCHES0);

    if (cc1)
        reg |= TCPC_REG_SWITCHES0_MEAS_CC1;
    else
        reg &= ~TCPC_REG_SWITCHES0_MEAS_CC1;

    if (cc2)
        reg |= TCPC_REG_SWITCHES0_MEAS_CC2;
    else
        reg &= ~TCPC_REG_SWITCHES0_MEAS_CC2;

    fusb302_tcpc_write(TCPC_REG_SWITCHES0, reg);

    delay_1ms(1);

}

void fusb302_cc_pd_switch(bool cc1, bool cc2) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_SWITCHES0);

    if (cc1)
        reg |= TCPC_REG_SWITCHES0_CC1_PD_EN;
    else
        reg &= ~TCPC_REG_SWITCHES0_CC1_PD_EN;

    if (cc2)
        reg |= TCPC_REG_SWITCHES0_CC2_PD_EN;
    else
        reg &= ~TCPC_REG_SWITCHES0_CC2_PD_EN;

    fusb302_tcpc_write(TCPC_REG_SWITCHES0, reg);

}

//Switches 1
void fusb302_autocrc_config(bool en, bool pwr_role, uint8_t spec_rev, bool datarole) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_SWITCHES1);

    if (en)
        reg |= TCPC_REG_SWITCHES1_AUTO_GCRC;
    else
        reg &= ~TCPC_REG_SWITCHES1_AUTO_GCRC;

    if (pwr_role)
        reg |= TCPC_REG_SWITCHES1_POWERROLE;
    else
        reg &= ~TCPC_REG_SWITCHES1_POWERROLE;

    if (datarole)
        reg |= TCPC_REG_SWITCHES1_DATAROLE;
    else
        reg &= ~TCPC_REG_SWITCHES1_DATAROLE;

    if (spec_rev & 1)
        reg |= TCPC_REG_SWITCHES1_SPECREV0;
    else
        reg &= ~TCPC_REG_SWITCHES1_SPECREV0;

    if (spec_rev & 2)
        reg |= TCPC_REG_SWITCHES1_SPECREV1;
    else
        reg &= ~TCPC_REG_SWITCHES1_SPECREV1;

    fusb302_tcpc_write(TCPC_REG_SWITCHES1, reg);

}

void fusb302_cc_bmc_en(bool cc1, bool cc2) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_SWITCHES1);

    if (cc1)
        reg |= TCPC_REG_SWITCHES1_TXCC1_EN;
    else
        reg &= ~TCPC_REG_SWITCHES1_TXCC1_EN;

    if (cc2)
        reg |= TCPC_REG_SWITCHES1_TXCC2_EN;
    else
        reg &= ~TCPC_REG_SWITCHES1_TXCC2_EN;

    fusb302_tcpc_write(TCPC_REG_SWITCHES1, reg);

}

//MEASURE
void fusb302_vbus_meas_en(bool en) {

    if (en)
        fusb302_tcpc_write(TCPC_REG_MEASURE, TCPC_REG_MEASURE_VBUS);
    else
        fusb302_tcpc_write(TCPC_REG_MEASURE, 0);

    delay_1ms(1);

}

void fusb302_mdac_set(uint8_t val) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_MEASURE);

    fusb302_tcpc_write(TCPC_REG_MEASURE, (reg & 0xC0) | (val & 0x3F));

    delay_1ms(1);

}

//Control0
void fusb302_cc_pu_current(uint8_t current) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_CONTROL0);

    reg &= 0xF3;
    reg |= ((current << 2) & 0x0C);

    fusb302_tcpc_write(TCPC_REG_CONTROL0, reg);


}

void fusb302_tx_fifo_flush(void) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_CONTROL0);

    reg |= TCPC_REG_CONTROL0_TX_FLUSH;

    fusb302_tcpc_write(TCPC_REG_CONTROL0, reg);

}

void fusb302_auto_pre(bool en) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_CONTROL0);

    if (en)
        reg |= TCPC_REG_CONTROL0_AUTO_PRE;
    else
        reg &= ~TCPC_REG_CONTROL0_AUTO_PRE;

    fusb302_tcpc_write(TCPC_REG_CONTROL0, reg);

}

void fusb302_int_en(bool en) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_CONTROL0);
    if (en)
        reg &= ~TCPC_REG_CONTROL0_INT_MASK;
    else
        reg |= TCPC_REG_CONTROL0_INT_MASK;

    fusb302_tcpc_write(TCPC_REG_CONTROL0, reg);
}

//Control1
void fusb302_sop_prime_en(bool en) {

    uint8_t reg = fusb302_tcpc_read(TCPC_REG_CONTROL1);

    if (en) {
        reg |= TCPC_REG_CONTROL1_ENSOP1;
        reg |= TCPC_REG_CONTROL1_ENSOP2;
    } else {
        reg &= ~TCPC_REG_CONTROL1_ENSOP1;
        reg &= ~TCPC_REG_CONTROL1_ENSOP2;
    }
    fusb302_tcpc_write(TCPC_REG_CONTROL1, reg);

}

void fusb302_rx_fifo_flush(void) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_CONTROL1);
    reg |= TCPC_REG_CONTROL1_RX_FLUSH;
    fusb302_tcpc_write(TCPC_REG_CONTROL1, reg);
}

//Control3
void fusb302_send_hardreset(void) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_CONTROL3);
    reg |= TCPC_REG_CONTROL3_SEND_HARDRESET;
    fusb302_tcpc_write(TCPC_REG_CONTROL3, reg);
}

void fusb302_set_automode(bool retry, uint8_t nretry, bool hrst, bool srst) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_CONTROL3);
    if (retry)
        reg |= TCPC_REG_CONTROL3_AUTO_RETRY;
    else
        reg &= ~TCPC_REG_CONTROL3_AUTO_RETRY;
    if (hrst)
        reg |= TCPC_REG_CONTROL3_AUTO_HARDRESET;
    else
        reg &= ~TCPC_REG_CONTROL3_AUTO_HARDRESET;
    if (srst)
        reg |= TCPC_REG_CONTROL3_AUTO_SOFTRESET;
    else
        reg &= ~TCPC_REG_CONTROL3_AUTO_SOFTRESET;
    reg &= ~TCPC_REG_CONTROL3_N_RETRIES;
    reg |= (nretry & TCPC_REG_CONTROL3_N_RETRIES);
    fusb302_tcpc_write(TCPC_REG_CONTROL3, reg);
}

//Status0
bool fusb302_is_vbusok(void) { //Occurred when VBUS over 4.0V
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_STATUS0);
    if(reg & TCPC_REG_STATUS0_VBUSOK){
        return TRUE;
    }
    return FALSE;
}

bool fusb302_is_cc_busy(void) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_STATUS0);
    if(reg & TCPC_REG_STATUS0_ACTIVITY){
        return TRUE;
    }
    return FALSE;
}

bool fusb302_get_cc_comp(void) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_STATUS0);
    if(reg & TCPC_REG_STATUS0_COMP){
        return TRUE;
    }
    return FALSE;
}

bool fusb302_is_crc_correct(void) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_STATUS0);
    if(reg & TCPC_REG_STATUS0_CRC_CHK){
        return TRUE;
    }
    return FALSE;
}

bool fusb302_is_fifo_full(void) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_STATUS0);
    if(reg & TCPC_REG_STATUS0_ALERT){
        return TRUE;
    }
    return FALSE;
}

bool fusb302_is_device_present(void) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_STATUS0);
    if(reg & TCPC_REG_STATUS0_WAKE){
        return TRUE;
    }
    return FALSE;
}

uint8_t fusb302_get_bc_lvl(void) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_STATUS0);
    if(reg & TCPC_REG_STATUS0_BC_LVL){
        return TRUE;
    }
    return FALSE;
}

//Status1
bool fusb302_is_rx_empty(void) {
    uint8_t reg = fusb302_tcpc_read(TCPC_REG_STATUS1);
    if(reg & TCPC_REG_STATUS1_RX_EMPTY){
        return TRUE;
    }
    return FALSE;
}

//Mask,Interrupt
uint32_t fusb302_get_interrupt_reason(void) {
    uint8_t res;
    uint32_t return_val;
    
    res = fusb302_tcpc_read(TCPC_REG_INTERRUPTB);
    return_val = ((res & 0xFFl) << 16);

    res = fusb302_tcpc_read(TCPC_REG_INTERRUPTA);
    return_val |= ((res & 0xFF) << 8);

    res = fusb302_tcpc_read(TCPC_REG_INTERRUPT);
    return_val |= (res & 0xFF);

    return return_val;
}

void fusb302_set_interrupt_mask(uint32_t mask) {

    fusb302_tcpc_write(TCPC_REG_MASKB, !(mask & 0x10000));
    fusb302_tcpc_write(TCPC_REG_MASKA, ~((mask >> 8) & 0xFF));
    fusb302_tcpc_write(TCPC_REG_MASK, ~(mask & 0xFF));

}

ufp_cc_status fusb302_get_ufp_cc_status(uint8_t cc_line) { //cc_line = 1 or 2

    uint8_t cc_lvl;
    bool comp;

    if (cc_line == 1)
        fusb302_cc_meas_switch(TRUE, FALSE);
    else if (cc_line == 2)
        fusb302_cc_meas_switch(FALSE, TRUE);
    else
        return UFP_CC_STATUS_ERROR;

    cc_lvl = fusb302_get_bc_lvl();

    // char out[50];
    // sprintf(out, "CC Line %d", cc_line);
    // USART_SendBytes(out);

    switch (cc_lvl) {
        case 0:
            // USART_SendBytes(" Status: UFP_CC_STATUS_NOT_CONN\n");
            return UFP_CC_STATUS_NOT_CONN;
        case 1:
            // USART_SendBytes(" Status: UFP_CC_STATUS_vRd_USB\n");
            return UFP_CC_STATUS_vRd_USB;
        case 2:
            // USART_SendBytes(" Status: UFP_CC_STATUS_vRd_1_5\n");
            return UFP_CC_STATUS_vRd_1_5;
        case 3:
            comp = fusb302_get_cc_comp();
            if (!comp) {
                // USART_SendBytes(" Status: UFP_CC_STATUS_vRd_3_0\n");
                return UFP_CC_STATUS_vRd_3_0;
            }

        default:
            return UFP_CC_STATUS_ERROR;

    }

}

ufp_cc_status fusb302_ufp_auto_polarity(void) {
    fusb302_ufp_cc = 0;
    delay_1ms(1);
    if (fusb302_is_vbusok()) {
        msg_id = 0;
        // USART_SendBytes("UFP Attached.\n");

        if (!is_cc_configured) {
            ufp_cc_status cc1_status = fusb302_get_ufp_cc_status(1);
            ufp_cc_status cc2_status = fusb302_get_ufp_cc_status(2);

            if (cc1_status == UFP_CC_STATUS_ERROR || cc1_status == UFP_CC_STATUS_ERROR)
                return UFP_CC_STATUS_ERROR;

            if (cc1_status > cc2_status) {
                fusb302_cc_bmc_en(TRUE, FALSE);
                fusb302_cc_meas_switch(TRUE, FALSE);
                fusb302_ufp_cc = 1;
                // USART_SendBytes("CC1 BMC EN\n");
                fusb302_tcpc_write(TCPC_REG_RESET, TCPC_REG_RESET_PD_RESET);
                return cc1_status;
            } else if (cc2_status > cc1_status) {
                fusb302_cc_bmc_en(FALSE, TRUE);
                fusb302_cc_meas_switch(FALSE, TRUE);
                fusb302_ufp_cc = 2;
                // USART_SendBytes("CC2 BMC EN\n");
                fusb302_tcpc_write(TCPC_REG_RESET, TCPC_REG_RESET_PD_RESET);
                return cc2_status;
            }
            is_cc_configured = TRUE;
        }
    } else {
        USART_SendBytes("Not Attached. \n");
        fusb302_cc_bmc_en(FALSE, FALSE);
        is_cc_configured = FALSE;
        return UFP_CC_STATUS_NOT_CONN;
    }
    return UFP_CC_STATUS_ERROR;
}

void USB302_Read_FIFO(uint8_t *data, uint8_t len)
{
    if(len){
        IIC_ReadSlave(FUSB302_I2C_SLAVE_ADDR, TCPC_REG_FIFOS, data, len);
    }
}

bool fusb302_is_connect_cc1(void){
    if(fusb302_ufp_cc == 1){
        return TRUE;
    }else {
        return FALSE;
    }
}

bool fusb302_is_connect_cc2(void){
    if(fusb302_ufp_cc == 2){
        return TRUE;
    }else {
        return FALSE;
    }
}

//FIFO
uint8_t fusb302_get_message(uint16_t* head, uint32_t* payload) {

    uint8_t token = fusb302_tcpc_read(TCPC_REG_FIFOS) & 0xe0;

    if (token > 0x40) {

        USB302_Read_FIFO((uint8_t *)head, 2);
        uint8_t len = PD_HEADER_CNT(*head);
        USB302_Read_FIFO((uint8_t *)payload, len * 4);
    }
    fusb302_rx_fifo_flush();
    return token;

}

void fusb302_send_message(uint16_t head, uint32_t* payload, uint8_t sop) {
    head &= 0xF1FF;
    msg_id++;
    if (msg_id == 8){
        msg_id = 0;
    }
    head |= msg_id << 9;
    uint8_t send_buf[49];
    uint8_t cnt = 0;
    send_buf[cnt++] = TCPC_REG_FIFOS;
    switch (sop) {
        case 0: //SOP
            send_buf[cnt++] = FUSB302_TKN_SYNC1;
            send_buf[cnt++] = FUSB302_TKN_SYNC1;
            send_buf[cnt++] = FUSB302_TKN_SYNC1;
            send_buf[cnt++] = FUSB302_TKN_SYNC2;
            break;
        case 1: //SOP'
            send_buf[cnt++] = FUSB302_TKN_SYNC1;
            send_buf[cnt++] = FUSB302_TKN_SYNC1;
            send_buf[cnt++] = FUSB302_TKN_SYNC3;
            send_buf[cnt++] = FUSB302_TKN_SYNC3;
            break;
        case 2: //SOP''
            send_buf[cnt++] = FUSB302_TKN_SYNC1;
            send_buf[cnt++] = FUSB302_TKN_SYNC3;
            send_buf[cnt++] = FUSB302_TKN_SYNC1;
            send_buf[cnt++] = FUSB302_TKN_SYNC3;
            break;
        default:
            break;
    }
    uint8_t total_len = PD_HEADER_CNT(head) * 4 + 2;
    send_buf[cnt++] = FUSB302_TKN_PACKSYM | total_len;

    send_buf[cnt++] = head & 0xff;
    send_buf[cnt++] = (head >> 8) & 0xff;

    memcpy(&send_buf[cnt], payload, total_len - 2);

    cnt += total_len - 2;

    send_buf[cnt++] = FUSB302_TKN_JAMCRC;
    send_buf[cnt++] = FUSB302_TKN_EOP;
    send_buf[cnt++] = FUSB302_TKN_TXON;
    fusb302_tcpc_write(TCPC_REG_CONTROL0, 0x40);
    IIC_WriteSlave(FUSB302_I2C_SLAVE_ADDR, send_buf, total_len + 9);
    fusb302_tcpc_write(TCPC_REG_CONTROL0, 0x05);

}
